package com.gunncs.actoriface;
import com.gunncs.actoriface.*;
import java.util.*;
import java.io.*;

public class SimpleNavigator implements Actor {
  protected static boolean adjusting;    
  public static final int lfront = 1;
  public static final int rfront = 0;
  public static final int lback = 2;
  public static final int rback = 3;
  
  public double[] sensors = new double[5];
  public static final double tol = 1.;
  
  public SimpleNavigator(){
    adjusting = false;
  }
  
  public String getMaze() {
    return "Testing"; // TODO(kroo): some one needs to do something asdfasdf
  }
  
  public void updateSensors(Sensor[] s) {
    int i = 0;
    for(int z=0; z<s.length; z++) {
      Sensor sensor = s[z];
      double val = sensor.getState()[0];
      if(sensor.getName().equals("RangeFinder")) {
        sensors[i] = val;
        i++;
//  Simulator.out.println("RangeFinder found (val = " + val + ")");
      } else {
//  Simulator.out.println("another type of sensor found and ignored: " + sensor.getName());
      }
    }
  }

  public void act(Robot r) {
    try {

    } catch (Exception e) {}
    if(noVals()){
      Simulator.out.println("no vals");
      // do nothing
    } else if (atTurn()) {
      boolean left = sensors[lfront] > sensors[rfront];


      adjusting=false;
     
    } else if(pointingR()) {
      Simulator.out.println("pointing R()");
      adjusting=true;
      r.turnLeft();
      r.waitAngle(1);
    } else if(pointingL()) {
      Simulator.out.println("pointing L()");
      adjusting=true;
      r.turnRight();
      r.waitAngle(1);
    } else if(false) {
      return;
    } else if(pointingCenter()) {
      Simulator.out.println("centered");
      good(r);
    } else if(onLeftSide()) {
      adjusting=true;
      Simulator.out.println("onleftside");
      r.turnRight();
      r.waitAngle(5);
      r.goForward();
      r.waitDistance(1);
      r.turnLeft();
      r.waitAngle(5);
      // turn right, drive until in middle, turn to orient straight
    } else if(onRightSide()) {
      adjusting=true;
      Simulator.out.println("onrightside");
      r.turnLeft();
      r.waitAngle(5);
      r.goForward();
      r.waitDistance(1);
      r.turnRight();
      r.waitAngle(5);
    } else {
      adjusting=false;
      Simulator.out.println("failed to act (something is wrong, im confused)");
      r.goForward();
      r.waitDistance(1);
      // WHOT
    }
  }

  public boolean pointingR() {
    double tol = 3;
    double leftside = sensors[lback] - sensors[lfront]; // front > back: pointing right
    double rightside = sensors[rfront] - sensors[rback]; // 
    boolean result = (leftside <= -tol && rightside <= -tol); 
    Simulator.out.println((result?"(pointing right) ":"(not pointing right) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +leftside + " " + rightside);
    return result;
  }

  public boolean pointingL() {
  double tol = 3;
    double leftside = sensors[lback] - sensors[lfront];
    double rightside = sensors[rfront] - sensors[rback];
    boolean result = (leftside >= tol && rightside >= tol); 
    Simulator.out.println((result?"(pointing left) ":"(not pointing left) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +leftside + " " + rightside);
    return result;
  }

  public boolean pointingCenter() {
    double tol = 2;
    double backside = sensors[lback] - sensors[rback];
    double frontside = sensors[lfront] - sensors[rfront];
    boolean result = (Math.abs(backside) <= tol && Math.abs(frontside) <= tol);
    Simulator.out.println((result?"(centered) ":"(not centered) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
    return result;
  }

  public boolean onRightSide() {
    double tol = 2;
    double backside = sensors[lback] - sensors[rback];
    double frontside = sensors[lfront] - sensors[rfront];
    boolean result = (backside > tol && frontside > tol);
    Simulator.out.println(result?"(right side) ":"(not right side) " + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
    return result;
  }

  public boolean onLeftSide() {
    double tol = 2;
    double backside = sensors[lback] - sensors[rback];
    double frontside = sensors[lfront] - sensors[rfront];
    boolean result = (backside < -tol && frontside < -tol);
    Simulator.out.println(result?"(left side) ":"(not left side) " + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
    return result;
  }

  /**
   * Everything is rose-y, continue going straight.
   */
  public void good(Robot r) {
    r.goForward();
    r.waitDistance(1);
  }


  public boolean goingToLCrash() {
    return(sensors[lfront] < tol *2 );
  }
  public boolean goingToRCrash() {
    return(sensors[rfront] < tol *2);
  }

  public boolean noVals() {
    //Simulator.out.println("NO VALS: " + sensors[rfront] + " " + sensors[lfront]);
    if (sensors[rfront] == 0 
        && sensors[lfront] == 0 
        && sensors[rback] == 0
        && sensors[lback] == 0)return true;
    else return false;
  }

  public boolean goingToCenterL() { //going towards the left
    return(sensors[rfront] > sensors[rback] && sensors[lfront] > sensors[rfront]);
      
  }

  public boolean goingToCenterR() { //going towards the right
    return(sensors[lfront] > sensors[lback] && sensors[rfront] > sensors[lfront]);
  }
  
  public boolean atCenter() { //note: checks if bot is in center, not if bot is pointing forwards
    return(Math.abs(sensors[rfront] + sensors[rback] - sensors[lfront] - sensors[lback]) < tol);
     
  }

  public boolean atTurn() {
    double tol = .5;
    double
     rf = sensors[rfront] + 1,
     lb = sensors[lback] + 1,
     lf = sensors[lfront] + 1,
     rb = sensors[rback] + 1;

    double ratio1 = rf / lb;
    double ratio2 = rb / lf;
    boolean result = !(Math.abs(ratio1 - ratio2) < tol);
    
    Simulator.out.println((result?"(at turn) ":"(not at turn) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " + ratio1 + " " + ratio2);

    return result;
  }
}